﻿/*
 * Author: Gabriel Reiser
 * Contributors:
 */

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Irrlicht.Net.Core
{
    [Serializable]
    public struct Quaternion
    {
        #region Private variables

        const float EPSILON = 1e-03f;

        public float w, x, y, z;

        private static readonly Quaternion identity = new Quaternion(1.0f, 0.0f, 0.0f, 0.0f);
        private static readonly Quaternion zero = new Quaternion(0.0f, 0.0f, 0.0f, 0.0f);
        private static readonly int[] next = new int[3] { 1, 2, 0 };

        #endregion

        #region Constructors

        

        /// <summary>
        ///             Creates a new Quaternion.
        /// </summary>
        public Quaternion(float x, float y, float z, float w)
        {
            this.w = w;
            this.x = x;
            this.y = y;
            this.z = z;
        }

        

        #endregion

        #region Operator Overloads

        
        public static Quaternion Multiply(Quaternion left, Quaternion right)
        {
            return left * right;
        }

        
        public static Quaternion operator *(Quaternion left, Quaternion right)
        {
            Quaternion q = new Quaternion();

            q.w = left.w * right.w - left.x * right.x - left.y * right.y - left.z * right.z;
            q.x = left.w * right.x + left.x * right.w + left.y * right.z - left.z * right.y;
            q.y = left.w * right.y + left.y * right.w + left.z * right.x - left.x * right.z;
            q.z = left.w * right.z + left.z * right.w + left.x * right.y - left.y * right.x;

            return q;
        }


        
        public static Vector3D Multiply(Quaternion quat, Vector3D vector)
        {
            return quat * vector;
        }

        public static Vector3D operator *(Quaternion quat, Vector3D vector)
        {
            // nVidia SDK implementation
            Vector3D uv, uuv;
            Vector3D qvec = new Vector3D(quat.x, quat.y, quat.z);

            uv = qvec.Cross(vector);
            uuv = qvec.Cross(uv);
            uv *= (2.0f * quat.w);
            uuv *= 2.0f;

            return vector + uv + uuv;

            // get the rotation matrix of the Quaternion and multiply it times the vector
            //return quat.ToRotationMatrix() * vector;
        }

        
        public static Quaternion Multiply(float scalar, Quaternion right)
        {
            return scalar * right;
        }

        
        public static Quaternion operator *(float scalar, Quaternion right)
        {
            return new Quaternion(scalar * right.w, scalar * right.x, scalar * right.y, scalar * right.z);
        }

        
        public static Quaternion Multiply(Quaternion left, float scalar)
        {
            return left * scalar;
        }

        
        public static Quaternion operator *(Quaternion left, float scalar)
        {
            return new Quaternion(scalar * left.w, scalar * left.x, scalar * left.y, scalar * left.z);
        }

        
        public static Quaternion Add(Quaternion left, Quaternion right)
        {
            return left + right;
        }

        
        public static Quaternion operator +(Quaternion left, Quaternion right)
        {
            return new Quaternion(left.w + right.w, left.x + right.x, left.y + right.y, left.z + right.z);
        }

        
        public static Quaternion operator -(Quaternion right)
        {
            return new Quaternion(-right.w, -right.x, -right.y, -right.z);
        }

        public static bool operator ==(Quaternion left, Quaternion right)
        {
            return (left.w == right.w && left.x == right.x && left.y == right.y && left.z == right.z);
        }

        public static bool operator !=(Quaternion left, Quaternion right)
        {
            return !(left == right);
        }

        #endregion

        #region Properties

        
        public static Quaternion Identity
        {
            get
            {
                return identity;
            }
        }

        
        public static Quaternion Zero
        {
            get
            {
                return zero;
            }
        }

        
        public float Norm
        {
            get
            {
                return x * x + y * y + z * z + w * w;
            }
        }

        
        public Vector3D XAxis
        {
            get
            {
                float fTy = 2.0f * y;
                float fTz = 2.0f * z;
                float fTwy = fTy * w;
                float fTwz = fTz * w;
                float fTxy = fTy * x;
                float fTxz = fTz * x;
                float fTyy = fTy * y;
                float fTzz = fTz * z;

                return new Vector3D(1.0f - (fTyy + fTzz), fTxy + fTwz, fTxz - fTwy);
            }
        }

        
        public Vector3D YAxis
        {
            get
            {
                float fTx = 2.0f * x;
                float fTy = 2.0f * y;
                float fTz = 2.0f * z;
                float fTwx = fTx * w;
                float fTwz = fTz * w;
                float fTxx = fTx * x;
                float fTxy = fTy * x;
                float fTyz = fTz * y;
                float fTzz = fTz * z;

                return new Vector3D(fTxy - fTwz, 1.0f - (fTxx + fTzz), fTyz + fTwx);
            }
        }

        
        public Vector3D ZAxis
        {
            get
            {
                float fTx = 2.0f * x;
                float fTy = 2.0f * y;
                float fTz = 2.0f * z;
                float fTwx = fTx * w;
                float fTwy = fTy * w;
                float fTxx = fTx * x;
                float fTxz = fTz * x;
                float fTyy = fTy * y;
                float fTyz = fTz * y;

                return new Vector3D(fTxz + fTwy, fTyz - fTwx, 1.0f - (fTxx + fTyy));
            }
        }

        #endregion

        #region Static methods

        public static Quaternion Slerp(float time, Quaternion quatA, Quaternion quatB)
        {
            return Slerp(time, quatA, quatB, false);
        }

        
        public static Quaternion Slerp(float time, Quaternion quatA, Quaternion quatB, bool UseQuickPath)
        {
            float cos = quatA.Dot(quatB);

            float angle = (float)Math.Acos(cos);

            if (Math.Abs(angle) < EPSILON)
            {
                return quatA;
            }

            float sin = (float)Math.Sin(angle);
            float inverseSin = 1.0f / sin;
            float coeff0 = (float)Math.Sin((1.0f - time) * angle) * inverseSin;
            float coeff1 = (float)Math.Sin(time * angle) * inverseSin;

            Quaternion result;

            if (cos < 0.0f && UseQuickPath)
            {
                coeff0 = -coeff0;
                // taking the complement requires renormalisation
                Quaternion t = coeff0 * quatA + coeff1 * quatB;
                t.Normalize();
                result = t;
            }
            else
            {
                result = (coeff0 * quatA + coeff1 * quatB);
            }

            return result;
        }

        
        public static Quaternion FromAngleAxisInRadians(float angle, Vector3D axis)
        {
            Quaternion quat = new Quaternion();

            float halfAngle = 0.5f * angle;
            float sin = (float)Math.Sin(halfAngle);

            quat.w = (float)Math.Cos(halfAngle);
            quat.x = sin * axis.x;
            quat.y = sin * axis.y;
            quat.z = sin * axis.z;

            return quat;
        }


        
        public static Quaternion FromAngleAxisInDegrees(float angle, Vector3D axis)
        {
            Quaternion quat = new Quaternion();
            float angleD = angle * (float)(Math.PI / 180.0);

            float halfAngle = 0.5f * angleD;
            float sin = (float)Math.Sin(halfAngle);

            quat.w = (float)Math.Cos(halfAngle);
            quat.x = sin * axis.x;
            quat.y = sin * axis.y;
            quat.z = sin * axis.z;

            return quat;
        }


        public static Quaternion Squad(float t, Quaternion p, Quaternion a, Quaternion b, Quaternion q)
        {
            return Squad(t, p, a, b, q, false);
        }

        
        public static Quaternion Squad(float t, Quaternion p, Quaternion a, Quaternion b, Quaternion q, bool UseQuickPath)
        {
            float slerpT = 2.0f * t * (1.0f - t);

            
            Quaternion slerpP = Slerp(t, p, q, UseQuickPath);
            Quaternion slerpQ = Slerp(t, a, b);

            
            return Slerp(slerpT, slerpP, slerpQ);
        }

        #endregion

        #region Public methods

        
        public float Dot(Quaternion quat)
        {
            return this.w * quat.w + this.x * quat.x + this.y * quat.y + this.z * quat.z;
        }

        
        public void Normalize()
        {
            float factor = 1.0f / (float)Math.Sqrt(this.Norm);

            w = w * factor;
            x = x * factor;
            y = y * factor;
            z = z * factor;
        }

        
        public void ToAngleAxis(ref float angle, ref Vector3D axis)
        {
            // The quaternion representing the rotation is
            //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

            float sqrLength = x * x + y * y + z * z;

            if (sqrLength > 0.0f)
            {
                angle = 2.0f * (float)Math.Acos(w);
                float invLength = -(float)Math.Sqrt(sqrLength);
                axis.x = x * invLength;
                axis.y = y * invLength;
                axis.z = z * invLength;
            }
            else
            {
                angle = 0.0f;
                axis.x = 1.0f;
                axis.y = 0.0f;
                axis.z = 0.0f;
            }
        }

        
        public Matrix3D ToRotationMatrix()
        {
            Matrix3D rotation = new Matrix3D();

            float tx = 2.0f * this.x;
            float ty = 2.0f * this.y;
            float tz = 2.0f * this.z;
            float twx = tx * this.w;
            float twy = ty * this.w;
            float twz = tz * this.w;
            float txx = tx * this.x;
            float txy = ty * this.x;
            float txz = tz * this.x;
            float tyy = ty * this.y;
            float tyz = tz * this.y;
            float tzz = tz * this.z;

            rotation.m11 = 1.0f - (tyy + tzz);
            rotation.m12 = txy - twz;
            rotation.m13 = txz + twy;
            rotation.m21 = txy + twz;
            rotation.m22 = 1.0f - (txx + tzz);
            rotation.m23 = tyz - twx;
            rotation.m31 = txz - twy;
            rotation.m31 = tyz + twx;
            rotation.m33 = 1.0f - (txx + tyy);

            return rotation;
        }

        /// <summary>
        /// Computes the inverse of a Quaternion.
        /// </summary>
        /// <returns></returns>
        public Quaternion Inverse()
        {
            float norm = this.w * this.w + this.x * this.x + this.y * this.y + this.z * this.z;
            if (norm > 0.0f)
            {
                float inverseNorm = 1.0f / norm;
                return new Quaternion(this.w * inverseNorm, -this.x * inverseNorm, -this.y * inverseNorm, -this.z * inverseNorm);
            }
            else
            {
                // return an invalid result to flag the error
                return Quaternion.Zero;
            }
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="xAxis"></param>
        /// <param name="yAxis"></param>
        /// <param name="zAxis"></param>
        public void ToAxes(out Vector3D xAxis, out Vector3D yAxis, out Vector3D zAxis)
        {
            xAxis = new Vector3D();
            yAxis = new Vector3D();
            zAxis = new Vector3D();

            Matrix3D rotation = this.ToRotationMatrix();

            xAxis.x = rotation.m11;
            xAxis.y = rotation.m21;
            xAxis.z = rotation.m31;

            yAxis.x = rotation.m12;
            yAxis.y = rotation.m22;
            yAxis.z = rotation.m32;

            zAxis.x = rotation.m13;
            zAxis.y = rotation.m23;
            zAxis.z = rotation.m33;
        }

        
        public void FromAxes(Vector3D xAxis, Vector3D yAxis, Vector3D zAxis)
        {
            Matrix3D rotation = new Matrix3D();

            rotation.m11 = xAxis.x;
            rotation.m21 = xAxis.y;
            rotation.m31 = xAxis.z;

            rotation.m12 = yAxis.x;
            rotation.m22 = yAxis.y;
            rotation.m32 = yAxis.z;

            rotation.m13 = zAxis.x;
            rotation.m23 = zAxis.y;
            rotation.m33 = zAxis.z;

            // set this quaternions values from the rotation matrix built
            FromRotationMatrix(rotation);
        }

        
        public void FromRotationMatrix(Matrix3D matrix)
        {
            // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
            // article "Quaternion Calculus and Fast Animation".

            float trace = matrix.m11 + matrix.m22 + matrix.m33;

            float root = 0.0f;

            if (trace > 0.0f)
            {
                // |this.w| > 1/2, may as well choose this.w > 1/2
                root = (float)Math.Sqrt(trace + 1.0f);  // 2w
                this.w = 0.5f * root;

                root = 0.5f / root;  // 1/(4w)

                this.x = (matrix.m32 - matrix.m23) * root;
                this.y = (matrix.m13 - matrix.m31) * root;
                this.z = (matrix.m21 - matrix.m12) * root;
            }
            else
            {
                // |this.w| <= 1/2

                int i = 0;
                if (matrix.m22 > matrix.m11)
                    i = 1;
                if (matrix.m33 > matrix[i, i])
                    i = 2;

                int j = next[i];
                int k = next[j];

                root = (float)Math.Sqrt(matrix[i, i] - matrix[j, j] - matrix[k, k] + 1.0f);

                unsafe
                {
                    fixed (float* apkQuat = &this.x)
                    {
                        apkQuat[i] = 0.5f * root;
                        root = 0.5f / root;

                        this.w = (matrix[k, j] - matrix[j, k]) * root;

                        apkQuat[j] = (matrix[j, i] + matrix[i, j]) * root;
                        apkQuat[k] = (matrix[k, i] + matrix[i, k]) * root;
                    }
                }
            }
        }

        
        public Quaternion Log()
        {
            // BLACKBOX: Learn this
            // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then
            // log(q) = A*(x*i+y*j+z*k).  If sin(A) is near zero, use log(q) =
            // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.

            // start off with a zero quat
            Quaternion result = Quaternion.Zero;

            if (Math.Abs(w) < 1.0f)
            {
                float angle = (float)Math.Acos(w);
                float sin = (float)Math.Sin(angle);

                if (Math.Abs(sin) >= EPSILON)
                {
                    float coeff = angle / sin;
                    result.x = coeff * x;
                    result.y = coeff * y;
                    result.z = coeff * z;
                }
                else
                {
                    result.x = x;
                    result.y = y;
                    result.z = z;
                }
            }

            return result;
        }

        
        public Quaternion Exponent()
        {
            // If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then
            // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k).  If sin(A) is near zero,
            // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.

            float angle = (float)Math.Sqrt(x * x + y * y + z * z);
            float sin = (float)Math.Sin(angle);

            // start off with a zero quat
            Quaternion result = Quaternion.Zero;

            result.w = (float)Math.Cos(angle);

            if (Math.Abs(sin) >= EPSILON)
            {
                float coeff = sin / angle;

                result.x = coeff * x;
                result.y = coeff * y;
                result.z = coeff * z;
            }
            else
            {
                result.x = x;
                result.y = y;
                result.z = z;
            }

            return result;
        }

        #endregion

        #region Object overloads

        public override int GetHashCode()
        {
            return (int)x ^ (int)y ^ (int)z ^ (int)w;
        }
        public override bool Equals(object obj)
        {
            Quaternion quat = (Quaternion)obj;

            return quat == this;
        }


        #endregion
    }
}
